#include <mc_controller/pure_pursuit.h>

PurePursuit::PurePursuit(){
    LOG
    kpLinear_ = 0.5;
}


PurePursuit::~PurePursuit(){

}

// 计算局部目标点的速度
mc_status PurePursuit::computeTwist(geometry_msgs::PoseStamped local_goal, geometry_msgs::Twist& cmd_vel){
    mc_status status;
    double goal_x = local_goal.pose.position.x;
    double goal_y = local_goal.pose.position.y;

    // 前瞻距离
    double lookhead_distance = hypot(goal_x, goal_y);
    double ld_2 = lookhead_distance * lookhead_distance;
    // 横向误差（假设机器人本地坐标的路径点）
    double lateral_error = goal_y;

    if(isPositionReached(localOriginPose_, local_goal)){
        if(!isAngleReached(localOriginPose_, local_goal)){
            rotateToTGoal(localOriginPose_, local_goal, cmd_vel);
        }
    } 
    else{
        // 距离越远，速度越快
        cmd_vel.linear.x = kpLinear_ * lookhead_distance;  // 简单的PID控制
        cmd_vel.linear.y = 0.0;
        cmd_vel.angular.z = 2.0 * lateral_error * cmd_vel.linear.x / ld_2;
    }

    return status;
}